#include<iostream>
using namespace std;
#include<cmath>

#include"library.h"

/* householder(n,a,d,e):	tridiagonalize a real symmetric matrix by householder transformation
 * n:		dimension of the matrix
 * a[n*n]	the original matrix, will be overwritten in this function, end up to be the similarity transformation matrix S = S_0 ... S_{n-3}, so that S^top A S = tridiag
 * d[n]		the diagonal elements after tridiagonalization
 * e[n]		the sub-diagonal elements after tridiagonalization, e[0] will be set to 0.
 *
 * After tridiagonalization:
 * d0 e1
 * e1 d1 e2
 *    e2 d3 e3
 *       .......
 *          .......
 *             e_{n-1} d_{n-1}
 */
void householder(int n, double *a, double *d, double *e){
	int i,j,k,m;
	double v, denorm, K, y;
	double *u, *p, *q;
	u = new double [n];
	p = new double [n];
	q = new double [n];
	
	e[0]=0;

	for(i=0;i<n-2;i++){// tridiagonalize the 0, 1, ..., n-2 th column & row, succesively
		// ***************** calculate \vec{v}, \vec{u}
		v=0;
		for(j=i+1;j<n;j++){
			u[j] = a[i*n+j];// \vec{u} = \vec{v} temporarily
			v += a[i*n+j] * a[i*n+j];
		}
		v = sqrt(v);// sqrt( a^2_{i,i+1} + ... + a^2_{i,n} )
		if( fabs(v) < 1E-14){// skip one transformation
			d[i]=a[i*n+i]; e[i+1] = 0;
			continue;
		}

		denorm = 2*v*v;// denorm = sqrt( 2v^2 -+ 2a_{i+1,i} v )
		if( a[i*n+i+1] > 0 ){
		       denorm += 2* a[i*n+i+1] * v;
		       a[i*n+i+1] = - v;// P\vec{v} = +- v \vec{e}_i
		       a[(i+1)*n+i] = -v;
		}
		else{
			denorm += - 2* a[(i+1)*n+i] *v;
			a[i*n+i+1] = v;
			a[(i+1)*n+i] = v;
		}
		d[i]=a[i*n+i]; e[i+1] = a[i*n+i+1];// record d[i] and e[i+1]
		for(j=i+2;j<n;j++){// tridiagonalization of the ith column & row
			a[i*n+j] = 0;
			a[j*n+i] = 0;
		}
		denorm = sqrt(denorm);// denorm = sqrt( 2v^2 -+ 2a_{i+1,i} v )

		for(j=i+1;j<n;j++){// \vec{u} = \vec{v}/denorm -+ v/denorm \vec{e}_i
			u[j] /= denorm;
		}
		if( u[i+1] > 0 ) u[i+1] += v/denorm;
		else u[i+1] -= v/denorm;

		// ***************** calculate K
		K=0;//calculate K = \vec{u}^\top \vec{p}
		for(j=i+1;j<n;j++){// determine p
			y=0;
			for(k=i+1;k<n;k++) y += a[j*n+k] * u[k];
			p[j] = 2*y;// \vec{p} = 2 B \vec{u}
			K += p[j] * u[j];
		}

		// ***************** calculate \vec{q}
		for(j=i+1;j<n;j++){// determine \vec{q} = \vec{p} - K \vec{u}
			q[j] = p[j] - K*u[j];
		}

		// ***************** calculate PBP
		for(j=i+1;j<n;j++){// PBP = B - \vec{u}\vec{q}^\top - \vec{q}\vec{u}^\top
			for(k=i+1;k<n;k++){
				a[j*n+k] -= u[j]*q[k] + q[j]*u[k];
			}
		}

		for(j=i+1;j<n;j++) a[ i*n + j ] = u[j];//restore u, to calculate P0P1P2P3...P_{n-3}
		/* for test only
		cout<<"i="<<i<<"\t A = "<<endl;
		for(j=0;j<n;j++){
			for(k=0;k<n;k++){
				cout<<a[j*n+k]<<",";
			}
			cout<<endl;
		}
		*/
	}
	d[n-2]=a[(n-2)*n+n-2]; d[n-1] = a[(n-1)*n+n-1];
	e[n-1]=a[(n-1)*n+n-2];
	// ************** write P_0 P_1 ... P_{n-3} into matrix *a
	for(j=n-2;j<n;j++){// set the right-lowest 2x2 matrix as unit matrix
		for(k=n-2;k<n;k++){
			if(k==j)a[j*n+k]=1;
			else a[j*n+k]=0;
		}
		cout<<endl;
	}
	for(i=n-3;i>=0;i--){// calculate S=S_0 S1...S_{n-3} and stores it in a
		for(j=i+1;j<n;j++){// \vec{u}^\top B is stored in A[i][i+1]...A[i][n-1]
			y=0;// y= 2\sum_k u_k a_{kj}
			for(k=i+1;k<n;k++)
				y+= a[i*n+k]*a[k*n+j];
			y*=2;
			for(k=i+1;k<n;k++){// a_{ij} = a_{ij} - 2 u_i \sum_k u_k a_{kj}
				a[k*n + j] -= a[i*n+k]*y;
			}
		}
		for(j=i+1;j<n;j++){
			a[i*n+j]=0;// delete info of \vec{u}
			a[j*n+i]=0;
		}
		a[i*n+i]=1;
	}

	delete [] u,p,q;
}

int main(){

	srand(time(NULL));

	double B[3][3] = {{2,0,1},{0,2,1},{1,1,2}};

	int n=10,i,j,k;
	double tempdouble,y;
	double *a_house= new double [n*n];
	cout<<"The original matrix: "<<endl;
	for(i=0;i<n;i++){
		for(j=i;j<n;j++){
			tempdouble = (double)rand()/RAND_MAX;
			a_house[i*n+j] = tempdouble;
			a_house[j*n+i] = tempdouble;
		}
		for(j=0;j<n;j++){
			cout<<a_house[i*n+j]<<",";
		}
		cout<<endl;
	}
	cout<<"ready? Go!"<<endl;

	double *d= new double [n];
	double *e= new double [n];
	householder(n,a_house,d,e);

	double *a_mid = new double [n*n];
	for(i=0;i<n;i++){
		for(j=0;j<n;j++){
			y=a_house[i*n+j-1]*e[j] + a_house[i*n+j]*d[j];
			if(j<n-1) y += a_house[i*n+j+1]*e[j+1];
			a_mid[i*n+j] = y;
		}
	}
	cout<<"a_house * tridiag * a_house^top: "<<endl;
	for(i=0;i<n;i++){
		for(j=0;j<n;j++){
			y=0;
			for(k=0;k<n;k++)
				y += a_mid[i*n+k] * a_house[j*n+k];
			cout<<y<<",";
		}
		cout<<endl;
	}

	delete [] a_house, d, e;

	return 0;
}
